#!/usr/bin/env python

import time
import random
import rospy
import carla

# parked_locations = [
#     carla.Transform(carla.Location(x=-32.217659, y=204.627274, z=2),
#                     carla.Rotation(yaw=-1.600555)),
#     carla.Transform(carla.Location(x=23.811020, y=207.353546, z=2),
#                     carla.Rotation(yaw=-0.504334))
# ]

# parked_locations = [
#     carla.Transform(carla.Location(x=-10.342915, y=68.348015,  z=1),
#                     carla.Rotation(yaw=90.596336)),
# ]

parked_locations = [
    carla.Transform(carla.Location(x=132.35, y=-196.58,  z=2),
                    carla.Rotation(yaw=0)),
    carla.Transform(carla.Location(x=133.81, y=-193.36,  z=2),
                    carla.Rotation(yaw=0)),
     carla.Transform(carla.Location(x=125.71, y=-193.18,  z=2),
                    carla.Rotation(yaw=0)),
     carla.Transform(carla.Location(x=124.28, y=-200.39,  z=2),
                    carla.Rotation(yaw=0)),
]

class CarlaParkVehicle():
    """
    class responsable of:
        -spawning 3 vehicles of which one ego
        -interact with ROS and carla server
        -destroy the created objects
        -execute the parking manoeuvre
    """

    def __init__(self):
        """
        construct object CarlaParkVehicle with server connection and
        ros node initiation
        """
        self.client = carla.Client('localhost', 2000)
        self.client.set_timeout(10.0)
        # self.client.load_world('Town03')
        self.world = self.client.get_world()
        self.actor_list = []
        blueprint_library = self.world.get_blueprint_library()

        # create ego vehicle
        bp = random.choice(blueprint_library.filter('vehicle.tesla.model3'))
        self.ob_list = []
        # create 2 parked vehicles
        for pos in parked_locations:
            try:
                v = self.world.spawn_actor(bp, pos)
                self.actor_list.append(v)
                self.ob_list.append(v)
            except:
                continue
        
        print("----------------------")

    def destroy(self):
        """
        destroy all the actors
        """
        print('destroying actors')
        for actor in self.actor_list:
            actor.destroy()
        print('done.')


    def run(self):
        """
        main loop
        """
        while True:
            pass


# ==============================================================================
# -- main() --------------------------------------------------------------------
# ==============================================================================

def main():
    """
    Main function
    """
    ego_vehicle = CarlaParkVehicle()
    try:
        ego_vehicle.run()
    finally:
        if ego_vehicle is not None:
            ego_vehicle.destroy()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass
